#include "problem.h"
#include <iostream>

#include <Eigen/Core>
#include <ceres/ceres.h>
#include <ceres/rotation.h>

#include <cmath>
#include <vector>
using namespace Eigen;

//共面方程残差
struct CoPlaneResidual {
    CoPlaneResidual(const Vector3d X1, const Vector3d X2) : X1(X1), X2(X2) {}

    template <typename T> bool operator()(const T *uv, const T *r, T *residual) const {
        T X2_[3];
        T X2_p[3];
        X2_p[0] = T(X2.x());
        X2_p[1] = T(X2.y());
        X2_p[2] = T(X2.z());
        ceres::AngleAxisRotatePoint(r, X2_p, X2_);
        residual[0] = (X1[1] * X2_[2] - X1[2] * X2_[1])
                    - uv[0] * (X1[0] * X2_[2] - X1[2] * X2_[0]) +
                      uv[1] * (X1[0] * X2_[1] - X1[1] * X2_[0]);
        return true;
    }

private:
    Vector3d X1, X2;
};

std::tuple<Vector3d, Matrix3d> relative_orientation(const DataRelativeOrientation &data) {
    // 1. 给定初值
    Vector3d B(data.Bx, 0, 0);
    Vector3d r = Vector3d::Zero(); // 采用 rodrigues 转角
    Vector2d uv(0, 0);
    double f = data.f;

    // 2. 采用矩阵行列式作为观测值求解

    //ceres 优化共面条件方程
    ceres::Problem problem;
    std::vector<ceres::ResidualBlockId> blocks;
    for (auto p : data.tiepoints) {
        ceres::CostFunction *cost = new ceres::AutoDiffCostFunction<CoPlaneResidual, 1, 2, 3>(
            new CoPlaneResidual({p.first.x(), p.first.y(), -f}, {p.second.x(), p.second.y(), -f}));
        problem.AddResidualBlock(cost, nullptr, uv.data(),r.data());
    }

    ceres::Solver::Options op;
    op.max_num_iterations = 100;
    op.linear_solver_type = ceres::DENSE_QR;
    ceres::Solver::Summary summary;
    ceres::Solve(op, &problem, &summary);
    //std::cout << summary.BriefReport() << "\n";
    // 3. 返回解
    Matrix3d R = rodrigues(r);
    B = Vector3d(B.x(), B.x() * uv(0), B.x() * uv(1));

    return std::tuple<Vector3d, Matrix3d>(B, R);
}

std::tuple<Vector3d, Matrix3d> relative_orientation_golden(const DataRelativeOrientation &_) {
    DataIntersection data = generate_intersection();
    Vector3d B = data.R_eo[0].transpose() * (data.X_eo[1] - data.X_eo[0]);
    Matrix3d R = data.R_eo[0].transpose() * data.R_eo[1];

    return std::make_tuple(B, R);
}
